#include "TimerOne.h"

// 引脚定义
#define LEFT_FRONT_DIR  7
#define LEFT_FRONT_STP  4
#define RIGHT_FRONT_DIR 5
#define RIGHT_FRONT_STP 2
#define RIGHT_BACK_DIR  6
#define RIGHT_BACK_STP  3
#define LEFT_BACK_DIR   10
#define LEFT_BACK_STP   9

// 底盘参数
#define WHEEL_RADIUS 0.075  // 轮子半径(m)
#define WHEEL_K 0.492      // 轮到中心的几何系数(m)
#define STEPS_PER_REV 200  // 步进电机一圈步数

// 电机控制参数
#define MIN_PULSE_INTERVAL 20  // 最小脉冲间隔(us)
#define INTERVAL_SCALE 15
#define MAX_SPEED 8.0         // 最大速度(m/s)
#define MAX_FREQ 40000.0      // 最大频率(Hz)

// 通信协议参数
#define CMD_HEADER1 0xAA      // 指令头第一字节
#define CMD_HEADER2 0xBB      // 指令头第二字节
#define CMD_FOOTER 0x55       // 指令尾
#define CMD_LENGTH 9          // 指令长度(字节): 头(2) + 数据(5) + 校验(1) + 尾(1)

// 全局变量
long FlagMotor1 = 0;          // 左前轮计数器
long FlagMotor2 = 0;          // 右前轮计数器
long FlagMotor3 = 0;          // 右后轮计数器
long FlagMotor4 = 0;          // 左后轮计数器

int Motor1Speed = 0;          // 左前轮速度
int Motor2Speed = 0;          // 右前轮速度
int Motor3Speed = 0;          // 右后轮速度
int Motor4Speed = 0;          // 左后轮速度

bool Motor1Dir = true;        // 左前轮方向
bool Motor2Dir = true;        // 右前轮方向
bool Motor3Dir = true;        // 右后轮方向
bool Motor4Dir = true;        // 左后轮方向

float Vx = 0;                 // X方向线速度
float Vy = 0;                 // Y方向线速度
float Wz = 0;                 // Z方向角速度

// 串口通信相关
byte cmdBuffer[32];           // 串口接收缓存
int cmdIndex = 0;             // 当前缓存索引
bool cmdStarted = false;      // 是否开始接收指令
bool cmdComplete = false;     // 指令是否完整
unsigned long lastByteTime = 0; // 最后一次接收字节的时间
#define RECEIVE_TIMEOUT 200    // 接收超时时间(ms)
#define BUFFER_SIZE 32         // 缓冲区大小

// 将线速度和角速度转换为各轮子速度
void velocityToWheelSpeed() {
  // 限制输入速度
  float limitedVx = constrain(Vx, -MAX_SPEED, MAX_SPEED);
  float limitedVy = constrain(Vy, -MAX_SPEED, MAX_SPEED);
  float limitedWz = constrain(Wz, -MAX_SPEED, MAX_SPEED);
  
  // 输出速度限制信息
  if (abs(Vx) > MAX_SPEED || abs(Vy) > MAX_SPEED || abs(Wz) > MAX_SPEED) {
    Serial.println("----speed too high, auto limit");
  }
  
  // 麦克纳姆轮运动学方程
  // ω1 = (1/r) * ( Vx - Vy - k*Wz ) 左前轮
  // ω2 = (1/r) * ( Vx + Vy + k*Wz ) 右前轮
  // ω3 = (1/r) * ( Vx - Vy + k*Wz ) 右后轮
  // ω4 = (1/r) * ( Vx + Vy - k*Wz ) 左后轮
  
  float w1 = (1.0/WHEEL_RADIUS) * (limitedVx - limitedVy - WHEEL_K*limitedWz);
  float w2 = (1.0/WHEEL_RADIUS) * (limitedVx + limitedVy + WHEEL_K*limitedWz);
  float w3 = (1.0/WHEEL_RADIUS) * (limitedVx - limitedVy + WHEEL_K*limitedWz);
  float w4 = (1.0/WHEEL_RADIUS) * (limitedVx + limitedVy - WHEEL_K*limitedWz);

  Serial.print("----Vx="); Serial.print(Vx);
  Serial.print(" limitedVx="); Serial.print(limitedVx);
  Serial.print("----Vy="); Serial.print(Vy);
  Serial.print(" limitedVy="); Serial.println(limitedVy);
  Serial.print("----Wz="); Serial.print(Wz);
  Serial.print(" limitedWz="); Serial.println(limitedWz);

  
  // 计算脉冲频率(Hz)
  float freq1 = abs(w1) * STEPS_PER_REV / (2.0*PI);
  float freq2 = abs(w2) * STEPS_PER_REV / (2.0*PI);
  float freq3 = abs(w3) * STEPS_PER_REV / (2.0*PI);
  float freq4 = abs(w4) * STEPS_PER_REV / (2.0*PI);
  
  // 限制最大频率，避免脉冲间隔计算问题
  freq1 = min(freq1, MAX_FREQ);
  freq2 = min(freq2, MAX_FREQ);
  freq3 = min(freq3, MAX_FREQ);
  freq4 = min(freq4, MAX_FREQ);
  
  // 转换为脉冲间隔(us)，最小值为20以避免超过定时器频率
  Motor1Speed = (freq1 > 0) ? max(1, (long)(1000000.0 / (freq1 * MIN_PULSE_INTERVAL * INTERVAL_SCALE))) : 0;
  Motor2Speed = (freq2 > 0) ? max(1, (long)(1000000.0 / (freq2 * MIN_PULSE_INTERVAL * INTERVAL_SCALE))) : 0;
  Motor3Speed = (freq3 > 0) ? max(1, (long)(1000000.0 / (freq3 * MIN_PULSE_INTERVAL * INTERVAL_SCALE))) : 0;
  Motor4Speed = (freq4 > 0) ? max(1, (long)(1000000.0 / (freq4 * MIN_PULSE_INTERVAL * INTERVAL_SCALE))) : 0;
  
  // 设置轮子旋转方向
  // 当小车向前行进时，左前轮和左后轮应该顺时针旋转，右前轮和右后轮应该逆时针旋转
  Motor1Dir = (w1 >= 0);  // 左前轮
  Motor2Dir = !(w2 >= 0);   // 右前轮,反转方向
  Motor3Dir = !(w3 >= 0);   // 右后轮,反转方向
  Motor4Dir = (w4 >= 0);  // 左后轮
  
  // 输出调试信息
  Serial.print("----2. get wheel speed w1=");
  Serial.print(w1); Serial.print(", w2=");
  Serial.print(w2); Serial.print(", w3=");
  Serial.print(w3); Serial.print(", w4=");
  Serial.println(w4);
  
  Serial.print("----3. get pulse interval us: ");
  Serial.print(Motor1Speed); Serial.print(", ");
  Serial.print(Motor2Speed); Serial.print(", ");
  Serial.print(Motor3Speed); Serial.print(", ");
  Serial.println(Motor4Speed);
  
  Serial.print("----4. get wheel direction: ");
  Serial.print(Motor1Dir ? "+" : "-"); Serial.print(", ");
  Serial.print(Motor2Dir ? "+" : "-"); Serial.print(", ");
  Serial.print(Motor3Dir ? "+" : "-"); Serial.print(", ");
  Serial.println(Motor4Dir ? "+" : "-");
}

void one_stepx()  // 步进电机脉冲控制中断函数
{
  FlagMotor1++;
  FlagMotor2++;
  FlagMotor3++;
  FlagMotor4++;
  
  // 左前轮控制
  if(FlagMotor1 >= Motor1Speed && Motor1Speed > 0) {
    FlagMotor1 = 0;
    digitalWrite(LEFT_FRONT_DIR, Motor1Dir);
    digitalWrite(LEFT_FRONT_STP, !digitalRead(LEFT_FRONT_STP)); // 翻转电平产生脉冲
  }
  
  // 右前轮控制
  if(FlagMotor2 >= Motor2Speed && Motor2Speed > 0) {
    FlagMotor2 = 0;
    digitalWrite(RIGHT_FRONT_DIR, Motor2Dir);
    digitalWrite(RIGHT_FRONT_STP, !digitalRead(RIGHT_FRONT_STP)); // 翻转电平产生脉冲
  }
  
  // 右后轮控制
  if(FlagMotor3 >= Motor3Speed && Motor3Speed > 0) {
    FlagMotor3 = 0;
    digitalWrite(RIGHT_BACK_DIR, Motor3Dir);
    digitalWrite(RIGHT_BACK_STP, !digitalRead(RIGHT_BACK_STP)); // 翻转电平产生脉冲
  }
  
  // 左后轮控制
  if(FlagMotor4 >= Motor4Speed && Motor4Speed > 0) {
    FlagMotor4 = 0;
    digitalWrite(LEFT_BACK_DIR, Motor4Dir);
    digitalWrite(LEFT_BACK_STP, !digitalRead(LEFT_BACK_STP)); // 翻转电平产生脉冲
  }
}

void setup() {
  // 串口初始化 - 降低波特率以提高稳定性
  Serial.begin(38400);
  
  // 引脚初始化
  pinMode(LEFT_FRONT_DIR, OUTPUT);
  pinMode(RIGHT_FRONT_DIR, OUTPUT);
  pinMode(RIGHT_BACK_DIR, OUTPUT);
  pinMode(LEFT_BACK_DIR, OUTPUT);
  pinMode(LEFT_FRONT_STP, OUTPUT);
  pinMode(RIGHT_FRONT_STP, OUTPUT);
  pinMode(RIGHT_BACK_STP, OUTPUT);
  pinMode(LEFT_BACK_STP, OUTPUT);
  
  // 初始化所有电机输出为低电平
  digitalWrite(LEFT_FRONT_STP, LOW);
  digitalWrite(RIGHT_FRONT_STP, LOW);
  digitalWrite(RIGHT_BACK_STP, LOW);
  digitalWrite(LEFT_BACK_STP, LOW);
  
  // 定时器初始化
  Timer1.initialize(MIN_PULSE_INTERVAL); // 20us中断时间
  Timer1.attachInterrupt(one_stepx); // 绑定中断函数
  
  // 初始化通信变量
  cmdIndex = 0;
  cmdStarted = false;
  cmdComplete = false;
  
  // 初始化电机速度和方向
  Motor1Speed = Motor2Speed = Motor3Speed = Motor4Speed = 0;
  Motor1Dir = Motor2Dir = Motor3Dir = Motor4Dir = true;
  
  // 初始化速度控制参数
  Vx = Vy = Wz = 0;
  
  Serial.println("----car4_controller.ino start");
}

void loop() {
  // 主要处理串口接收
  processSerialData();
  
  // 如果接收到完整指令则处理
  if (cmdComplete) {
    cmdComplete = false;
    
    // 验证校验和
    if (verifyChecksum()) {
      // 解析Vx, Vy, Wz参数
      parseCommand();
      
      // 输出调试信息
      Serial.print("---- 1. get velocity Vx=");
      Serial.print(Vx);
      Serial.print(", Vy=");
      Serial.print(Vy);
      Serial.print(", Wz=");
      Serial.println(Wz);
      
      // 计算各轮子速度
      velocityToWheelSpeed();
    } else {
      Serial.println("---- verifyChecksum failed");
    }
  }
}

// 处理串口数据
void processSerialData() {
  // 检查接收超时
  if (cmdStarted && (millis() - lastByteTime > RECEIVE_TIMEOUT)) {
    Serial.println("----timeout, reset receive state");
    cmdStarted = false;
    cmdIndex = 0;
  }
  
  // 读取串口数据
  while (Serial.available() > 0) {
    byte inByte = Serial.read();
    lastByteTime = millis(); // 更新最后接收时间
    
    // 如果收到指令头第一字节，总是重新开始
    if (inByte == CMD_HEADER1) {
      cmdStarted = true;
      cmdIndex = 0;
      cmdBuffer[cmdIndex++] = inByte;
      continue; // 继续处理下一个字节
    }
    
    // 如果没有开始接收，忽略其他字节
    if (!cmdStarted) {
      continue;
    }
    
    // 存储字节，防止缓冲区溢出
    if (cmdIndex < BUFFER_SIZE) {
      cmdBuffer[cmdIndex++] = inByte;
    } else {
      // 缓冲区溢出，重置状态
      Serial.println("----cache overflow");
      printCmdBuffer();
      cmdStarted = false;
      cmdIndex = 0;
      continue;
    }
    
    // 如果收到第二个字节，检查是否为指令头第二字节
    if (cmdIndex == 2 && cmdBuffer[1] != CMD_HEADER2) {
      // 不是有效的指令头，重置
      cmdStarted = false;
      cmdIndex = 0;
      continue;
    } else if (cmdIndex == 2 && cmdBuffer[1] == CMD_HEADER2) {
      // 收到有效的指令头
      Serial.println("----detect complete header");
    }
    
    // 如果收到指令尾，检查数据帧长度和格式
    if (inByte == CMD_FOOTER) {
      // 验证数据帧长度
      if (cmdIndex == CMD_LENGTH) {
        // 验证数据帧格式是否合理
        if (cmdBuffer[0] == CMD_HEADER1 && cmdBuffer[1] == CMD_HEADER2 && cmdBuffer[CMD_LENGTH-1] == CMD_FOOTER) {
          // 长度和格式都正确，指令完整
          cmdComplete = true;
          cmdStarted = false;
          Serial.println("----received complete command");
        } else {
          // 格式不正确，重置状态
          Serial.println("----cmd format error");
          printCmdBuffer();
          cmdStarted = false;
          cmdIndex = 0;
        }
      } else {
        // 长度不正确，指令不完整
        Serial.print("----cmd length error: ");
        Serial.println(cmdIndex);
        printCmdBuffer();
        
        // 尝试寻找下一个指令头
        bool foundHeader = false;
        for (int i = 1; i < cmdIndex - 1; i++) {
          if (cmdBuffer[i] == CMD_HEADER1 && i+1 < cmdIndex && cmdBuffer[i+1] == CMD_HEADER2) {
            // 在缓冲区中找到新的完整指令头，移动数据
            int newLength = cmdIndex - i;
            for (int j = 0; j < newLength; j++) {
              cmdBuffer[j] = cmdBuffer[i + j];
            }
            cmdIndex = newLength;
            foundHeader = true;
            Serial.println("----find new complete header");
            break;
          }
        }
        
        if (!foundHeader) {
          // 没有找到新的完整指令头，重置状态
          cmdStarted = false;
          cmdIndex = 0;
        }
      }
    }
  }
}

void printCmdBuffer() {
  Serial.print("----cmdBuffer: ");
  // 打印实际接收到的字节数，而不是固定的CMD_LENGTH
  int printLength = min(cmdIndex, BUFFER_SIZE);
  for (int i = 0; i < printLength; i++) {
    Serial.print("0x");
    if (cmdBuffer[i] < 16) Serial.print("0"); // 补零，保证两位16进制显示
    Serial.print(cmdBuffer[i], HEX);
    Serial.print(" ");
  }
  Serial.println();
}

// 验证校验和函数
bool verifyChecksum() {
  // 打印接收到的cmdBuffer内容
  Serial.print("----verifyChecksum: ");
  printCmdBuffer();
  
  // 校验和位于倒数第二个字节
  byte checksum = cmdBuffer[CMD_LENGTH - 2];
  byte calculatedChecksum = 0;
  
  // 计算校验和 (从头部到数据的异或校验)
  for (int i = 0; i < CMD_LENGTH - 2; i++) {
    calculatedChecksum ^= cmdBuffer[i];
  }
  
  // 打印校验结果
  Serial.print("----verifyChecksum: receive=");
  Serial.print(checksum, HEX);
  Serial.print(", calculate=");
  Serial.println(calculatedChecksum, HEX);
  
  return (checksum == calculatedChecksum);
}

// 解析16进制指令
void parseCommand() {
  // 指令格式: [头部(0xAA 0xBB)][Vx高][Vx低][Vy高][Vy低][Wz][校验和][尾(0x55)]
  // 解析Vx (2字节，有符号整数，单位: cm/s)
  int vx_raw = (cmdBuffer[2] << 8) | cmdBuffer[3];
  // 解析Vy (2字节，有符号整数，单位: cm/s)
  int vy_raw = (cmdBuffer[4] << 8) | cmdBuffer[5];
  // 解析Wz (1字节，有符号整数，单位: 0.1rad/s)
  int wz_raw = (int8_t)cmdBuffer[6];
  
  // 转换为实际物理单位 (m/s 和 rad/s)
  Vx = vx_raw / 100.0; // cm/s to m/s
  Vy = vy_raw / 100.0; // cm/s to m/s
  Wz = wz_raw / 10.0;   // 0.1rad/s to rad/s
}

// 生成响应数据
void sendResponse(const char* message) {
  Serial.println(message);
}
